int main() {

//	PRECHARGE_RELAY = 0;	// low turns on the precharge relay.
	clear_oc();	// It starts in unknown state. So, clear overcurrent.	

	InitUART2();

	while(1) {
	//	if (fault_bits & PRECHARGE_FAULT) {
	//		if (counter_1k >= 8000) { // 8 seconds
	//			fault_bits &= ~PRECHARGE_FAULT;  // clear the precharge fault.
	//			CONTACTOR_RELAY = 0; // close main contactor.
	//			PRECHARGE_RELAY = 1; // open the precharge relay. It should be basically 0 current, so OK to open.
	//		}
 	//	}
							
	// Now, analyze the fault bits, to see what to do!
	//	if (fault_bits) {
	//		if (fault_bits & VREF_FAULT) {	// This is really really bad.  Probably no hardware current limiting.  Just open contactors even if a lot of current is flowing through.  Contactor damage is worth it.
	//			CONTACTOR_RELAY = 1;
	//			PRECHARGE_RELAY = 1;
	//		}
	//		else if (fault_bits & THROTTLE_FAULT) {
	//			if (current_feedback < 10) { // make sure the current has fallen to no more than a small amount of amps before opening the contactors.  No sense messing up the contacts.
	//				CONTACTOR_RELAY = 1;
	//				PRECHARGE_RELAY = 1;
	//			}
	//		}
	//	}

		now = counter_2k;
		if (now - previous > 2000) {
			if (TransmitReady()) {
				SendCharacter(serialStream[counter1++]);
			}
			if (counter1 >= 8*26 ) {
				counter1 = 0;
				previous = now;

				variable[0] = fault_bits;
				variable[1] = i_b + 128;
				variable[2] = i_alpha + 128;
				variable[3] = i_beta + 128;
				variable[4] = i_d + 128;
				variable[5] = i_q + 128;
				variable[6] = v_d_small + 1024;
				variable[7] = v_q_small + 1024;
				variable[8] = v_alpha + 1024;
				variable[9] = v_beta + 1024;
				variable[10] = Vr1 + 1024;
				variable[11] = Vr2 + 1024;
				variable[12] = Vr3 + 1024;
				variable[13] = rotorFluxAngle;
				variable[14] = rotorPos;
				variable[15] = tempOffset1;
				variable[16] = tempOffset2;
				variable[17] = tickCount;
				variable[18] = T1;
				variable[19] = T2;
				variable[20] = Ta;
				variable[21] = Tb;
				variable[22] = Tc;
				variable[23] = throttle_feedback;
				variable[24] = totalTime;
				variable[25] = overCurrentHasOccurred;
				// Now convert all of these from integers to ascii.  Let's use hex.  It's way faster.
				
				for (i = 0; i < 25; i++) {
					for (j = 5; j > 2; j--) {
						streamOffset = (i << 3) + j;
						asciiChar = (variable[i] & 0b1111);
						variable[i] = (variable[i] >> 4);
						if (asciiChar > 9)
							serialStream[streamOffset] = (char)(55 + asciiChar);
						else 
							serialStream[streamOffset] = (char)(48 + asciiChar);
					}
				}

				asciiChar = variable[25] & 15;
				streamOffset = (25 << 3) + 3;
				serialStream[streamOffset] = 48 + asciiChar;
			}	
		}
		asm("CLRWDT");
	}
	return 0;
}

void clear_oc(void)
{
	OVER_CURRENT_TRIP_RESET = 0;				// OC clear low (low to clear)
	asm("nop"); asm("nop");				// 8 nops = 1/2 uS - enough for NAND I think. haha.
	asm("nop"); asm("nop"); 
	asm("nop"); asm("nop");				
	asm("nop"); asm("nop"); 
	OVER_CURRENT_TRIP_RESET = 1;				// OC clear high (high for normal operation)
}

// T1 and T2 are in the range [0, 1023].
// MAX_PWM = 2047
void CalcTimes(void) {
    Tc = (MAX_PWM - T1 - T2) >> 1;
    Tb = Tc + T1;
    Ta = Tb + T2;
}

// 
// Vr1, Vr2, and Vr3 are in the range [-1023, 1023].
// Vr1, Vr2, and Vr3 are the result of doing the "modified" inverse clarke transformation, where v_alpha and v_beta are swapped.
void CalcRefVec( void ) { 
	if( Vr1 >= 0 ) {       
        // (xx1)
        if( Vr2 >= 0 ) {
            // (x11)
            // Must be Sector 3 since Sector 7 not allowed
            // Sector 3: (0,1,1)  0-60 degrees
            T1 = Vr1; // Now we know that Vr2 and Vr1 is in [0, 1023].
            T2 = Vr2; // This is what the assembly says, but not the C.  Friggen retarded crap.  And I don't understand what they are doing either.  Horrible.
            CalcTimes();
            PDC1 = Ta;
            PDC2 = Tb;
            PDC3 = Tc;
        }
        else {            
            // (x01)
            if( Vr3 >= 0 ) {
                // Sector 5: (1,0,1)  120-180 degrees
                T2 = Vr1;
                T1 = Vr3;
                CalcTimes();
                PDC1 = Tc;
                PDC2 = Ta;
                PDC3 = Tb;
			}
            else {
                // Sector 1: (0,0,1)  60-120 degrees
                T2 = -Vr2;
                T1 = -Vr3;
                CalcTimes();
                PDC1 = Tb;
                PDC2 = Ta;
                PDC3 = Tc;
            }
        }
 	}       
    else {
        // (xx0)
        if( Vr2 >= 0 ) {
            // (x10)
            if( Vr3 >= 0 ) {
                // Sector 6: (1,1,0)  240-300 degrees
                T2 = Vr3;
                T1 = Vr2;
                CalcTimes();
                PDC1 = Tb;
                PDC2 = Tc;
                PDC3 = Ta;
            }
            else {
                // Sector 2: (0,1,0)  300-0 degrees
                T2 = -Vr3;
                T1 = -Vr1;
                CalcTimes();
                PDC1 = Ta;
                PDC2 = Tc;
                PDC3 = Tb;
            }
        }
        else {            
            // (x00)
            // Must be Sector 4 since Sector 0 not allowed
            // Sector 4: (1,0,0)  180-240 degrees
            T2 = -Vr1;
            T1 = -Vr2;
            CalcTimes();
            PDC1 = Tc;
            PDC2 = Tb;
            PDC3 = Ta;
        }
    }
}

void ComputeRotorFluxAngle() {
//;	 Physical form of equations:
//;  Magnetizing current (amps):
//;     Imag = Imag + (fLoopPeriod/fRotorTmConst)*(Id - Imag)
//;
//;  Slip speed in RPS:
//;     VelSlipRPS = (1/fRotorTmConst) * Iq/Imag / (2*pi)
//;
//;  Rotor flux speed in RPS:
//;     VelFluxRPS = iPoles * VelMechRPS + VelSlipRPS
//;
//;  Rotor flux angle (radians):
//;     AngFlux = AngFlux + fLoopPeriod * 2 * pi * VelFluxRPS

	// Compute Magnetizing current:
	// 108 = loopPeriodInSecond/rotorTimeConstant * 65536, where rotorTimeConstant = 0.078 and loopPeriod is 128us
	// magnetizingCurrent is in [-128, 128].  The same range as i_d
	magnetizingCurrent = ((magnetizingCurrent << 16) + (108L)*(i_d - magnetizingCurrent)) >> 16;
	
	// Compute the slip speed in revolutions per second, but scaled by 65536: 128*65536 would mean 7680 rpm.
	slipSpeed = (133717L * ((long)i_q)) / magnetizingCurrent;
	
	// rotor Flux Speed in revolutions per second, but scaled by 65536: 128*65536 would mean 7680 rpm.
	// mechanical speed is in revolutions per second, but scaled by 65536.
	rotorFluxSpeed = numberOfPolePairs*mechanicalSpeed + slipSpeed;

	// rotor flux angle is in radians, but scaled by 65536.
	rotorFluxAngle = rotorFluxAngle + ((53L*rotorFluxSpeed) >> 16);
}